#include "math.h"
#include "DVector.h"
#include "Quaternion.h"

namespace DinoMath
{
inline Quaternion qmix
		(
			Quaternion const & x, 
			Quaternion const & y, 
			float const & a
		);


inline Quaternion qmix
		(
			Quaternion const & x, 
			Quaternion const & y, 
			float const & a
		)
		{
			if(a <= float(0)) return x;
			if(a >= float(1)) return y;

			float fCos = dot(x, y);
			Quaternion y2(y); //BUG!!! tquat<T> y2;
			if(fCos < float(0))
			{
				y2 = -y;
				fCos = -fCos;
			}

			//if(fCos > 1.0f) // problem
			float k0, k1;
			if(fCos > float(0.9999))
			{
				k0 = float(1) - a;
				k1 = float(0) + a; //BUG!!! 1.0f + a;
			}
			else
			{
				float fSin = sqrt(float(1) - fCos * fCos);
				float fAngle = atan2(fSin, fCos);
				float fOneOverSin = float(1) / fSin;
				k0 = sin((float(1) - a) * fAngle) * fOneOverSin;
				k1 = sin((float(0) + a) * fAngle) * fOneOverSin;
			}

			return Quaternion(
				k0 * x.x + k1 * y2.x,
				k0 * x.y + k1 * y2.y,
				k0 * x.z + k1 * y2.z,
				k0 * x.w + k1 * y2.w
				);
		}
		inline vec3 mix
		(
			vec3 const & x, 
			vec3 const & y, 
			float const & a
		)
		{
			return vec3(
				vec3(x) + a * vec3(y - x));
		}

		inline vec3 lerp(const vec3& x, const vec3& y, float const &a) { return mix(x, y, a);}
	



		inline Quaternion mix
		(
			Quaternion const & x, 
			Quaternion const & y, 
			float const & a
		)
		{
			if(a <= float(0)) return x;
			if(a >= float(1)) return y;

			float fCos = dot(x, y);
			Quaternion y2(y); //BUG!!! tquat<T> y2;
			if(fCos < float(0))
			{
				y2 = -y;
				fCos = -fCos;
			}

			//if(fCos > 1.0f) // problem
			float k0, k1;
			if(fCos > float(0.9999))
			{
				k0 = float(1) - a;
				k1 = float(0) + a; //BUG!!! 1.0f + a;
			}
			else
			{
				float fSin = sqrt(float(1) - fCos * fCos);
				float fAngle = atan2(fSin, fCos);
				float fOneOverSin = float(1) / fSin;
				k0 = sin((float(1) - a) * fAngle) * fOneOverSin;
				k1 = sin((float(0) + a) * fAngle) * fOneOverSin;
			}

			return Quaternion(
				k0 * x.x + k1 * y2.x,
				k0 * x.y + k1 * y2.y,
				k0 * x.z + k1 * y2.z,
				k0 * x.w + k1 * y2.w
				);
		}
}
